#include "stdafx.h"
#include "MainApplication.h"

static float	gMaxEngineForce = 3000.f;

static float	gSteeringIncrement = 0.04f;
static float	gSteeringClamp = 0.8f;

static float	gWheelRadius = 0.5f;
static float	gWheelWidth = 0.4f;

static float	gWheelFriction = 1e30f;//1000;//1e30f;
static float	gSuspensionStiffness = 20.f;
static float	gSuspensionDamping = 2.3f;
static float	gSuspensionCompression = 4.4f;

static float	gRollInfluence = 0.1f;//1.0f;
static float	gSuspensionRestLength = 0.6f;
static float	gMaxSuspensionTravelCm = 500.0;
static float	gFrictionSlip = 10.5;

static const Ogre::Vector3    CameraStart            = Ogre::Vector3(300, 135, -16);
// -------------------------------------------------------------------------
static const Ogre::Vector3   robotPosition             = Ogre::Vector3(15, 30,-15);

#define CUBE_HALF_EXTENTS 1


//-------------------------------------------------------------------------------------
MainApplication::MainApplication(void)
{
	for (int i = 0; i < 4; i++)
    {
        mWheelsEngine[i] = 0;
		mWheelsSteerable[i] = 0;
    }
    mWheelsEngineCount = 2;
    mWheelsEngine[0] = 0;
    mWheelsEngine[1] = 1;

	mWheelsSteerableCount = 2;
    mWheelsSteerable[0] = 0;
    mWheelsSteerable[1] = 1;

    mSteeringLeft = false;
    mSteeringRight = false;

    mEngineForce = 0;
    mSteering = 0;
}
//-------------------------------------------------------------------------------------
MainApplication::~MainApplication(void)
{
}

//-------------------------------------------------------------------------------------
void MainApplication::createScene(void)
{
	setCamera();

	setStaticPlane();

	setRobot();

    setLight();
}

void MainApplication::setLight(void)
{
	// Set ambient light
    mSceneMgr->setAmbientLight(Ogre::ColourValue(0.0f,0.0f,0.0f));
    // Create a light
    Ogre::Light* light = mSceneMgr->createLight("MainLight");
	light->setType(Ogre::Light::LT_DIRECTIONAL);
    light->setDiffuseColour(Ogre::ColourValue(1.0f,1.0f,1.0f));
	light->setDirection(Ogre::Vector3(1,-1,0));
}

void MainApplication::setStaticPlane(void)
{
	tableEntity = mSceneMgr->createEntity("Table", "table2011.mesh");
    tableNode = mSceneMgr->getRootSceneNode()->createChildSceneNode();
    tableNode->attachObject(tableEntity);

	OgreBulletCollisions::StaticMeshToShapeConverter *trimeshConverter = new OgreBulletCollisions::StaticMeshToShapeConverter(tableEntity);
	OgreBulletCollisions::TriangleMeshCollisionShape *tableCollisionShape = trimeshConverter->createTrimesh();
	delete trimeshConverter;

	OgreBulletDynamics::RigidBody *tableBody = new OgreBulletDynamics::RigidBody("tableBody", mWorld);
	tableBody->setShape(tableNode, tableCollisionShape, 0.1f, 0.8f, 0.0f);

	/*Ogre::Plane plane(Vector3::UNIT_Y, 0);
	Ogre::MeshManager::getSingleton().createPlane("plane", ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
		plane, 1500,1500,20,20,true,1,5,5,Vector3::UNIT_Z);

	Ogre::Entity* tableEntity = mSceneMgr->createEntity("LightPlaneEntity", "plane");
	tableNode = mSceneMgr->getRootSceneNode()->createChildSceneNode();
	tableNode->attachObject(tableEntity);
	tableEntity->setMaterialName("Examples/BeachStones");

	CollisionShape *Shape = new StaticPlaneCollisionShape (Ogre::Vector3(0,1,0), 0);
    RigidBody *defaultPlaneBody = new RigidBody("Plane", mWorld);
    defaultPlaneBody->setStaticShape(Shape, 0.0f, 0.8f);*/
}

void MainApplication::setRobot(void)
{
	/*robotEntity = mSceneMgr->createEntity("Robot", "Robot.mesh");
    robotNode = tableNode->createChildSceneNode();
    robotNode->attachObject(robotEntity);

	// we need the bounding box of the box to be able to set the size of the Bullet-box
	Ogre::Vector3 robotSize = robotEntity->getBoundingBox().getSize(); 
	robotSize /= 2.0f; // only the half needed
	//robotSize *= 0.95f;	// Bullet margin is a bit bigger so we need a smaller size

	// after that create the Bullet shape with the calculated size
	OgreBulletCollisions::BoxCollisionShape *robotCollisionShape = new OgreBulletCollisions::BoxCollisionShape(robotSize);
	robotBody = new OgreBulletDynamics::RigidBody("robotBody", mWorld);
	robotBody->setShape(robotNode, robotCollisionShape, 0.0f, 0.0f, 2.0f, Ogre::Vector3(0, 100, 0));
	//robotBody->setLinearVelocity(mCamera->getDerivedDirection().normalisedCopy() * 7.0f ); // shooting speed*/

	// create robot
	const Ogre::Vector3 chassisShift(0, 1.0, 0);
	float connectionHeight = 0.7f;

	mChassis = mSceneMgr->createEntity("chassis", "chassis.mesh");

	SceneNode *node = mSceneMgr->getRootSceneNode ()->createChildSceneNode ();

	SceneNode *chassisnode = node->createChildSceneNode ();
	chassisnode->attachObject (mChassis);
	chassisnode->setPosition (chassisShift);

	mChassis->setCastShadows(true);

	BoxCollisionShape* chassisShape = new BoxCollisionShape(Ogre::Vector3(1.f,0.75f,2.1f));
	CompoundCollisionShape* compound = new CompoundCollisionShape();
	compound->addChildShape(chassisShape, chassisShift); 

	mCarChassis = new WheeledRigidBody("carChassis", mWorld);

	mCarChassis->setShape (node, compound, 0.6f, 0.6f, 800, robotPosition, Quaternion::IDENTITY);
	mCarChassis->setDamping(0.2f, 0.2f);

	mCarChassis->disableDeactivation ();
	mTuning = new VehicleTuning(
		gSuspensionStiffness,
		gSuspensionCompression,
		gSuspensionDamping,
		gMaxSuspensionTravelCm,
		gFrictionSlip);

	mVehicleRayCaster = new VehicleRayCaster(mWorld);
	mVehicle = new RaycastVehicle(mCarChassis, mTuning, mVehicleRayCaster);


	int rightIndex = 0;
	int upIndex = 1;
	int forwardIndex = 2;

	mVehicle->setCoordinateSystem(rightIndex, upIndex, forwardIndex);

	Ogre::Vector3 wheelDirectionCS0(0,-1,0);
	Ogre::Vector3 wheelAxleCS(-1,0,0);

	for (size_t i = 0; i < 4; i++)
	{
		mWheels[i] = mSceneMgr->createEntity("wheel" + StringConverter::toString(i), "wheel.mesh");
		mWheels[i]->setCastShadows(true);

		mWheelNodes[i] = mSceneMgr->getRootSceneNode ()->createChildSceneNode();
		mWheelNodes[i]->attachObject (mWheels[i]);
	}

	bool isFrontWheel = true;

	Ogre::Vector3 connectionPointCS0 (
		CUBE_HALF_EXTENTS-(0.3*gWheelWidth),
		connectionHeight,
		2*CUBE_HALF_EXTENTS-gWheelRadius);


	mVehicle->addWheel(
		mWheelNodes[0],
		connectionPointCS0,
		wheelDirectionCS0,
		wheelAxleCS,
		gSuspensionRestLength,
		gWheelRadius,
		isFrontWheel, gWheelFriction, gRollInfluence);

	connectionPointCS0 = Ogre::Vector3(
		-CUBE_HALF_EXTENTS+(0.3*gWheelWidth),
		connectionHeight,
		2*CUBE_HALF_EXTENTS-gWheelRadius);


	mVehicle->addWheel(
		mWheelNodes[1],
		connectionPointCS0,
		wheelDirectionCS0,
		wheelAxleCS,
		gSuspensionRestLength,
		gWheelRadius,
		isFrontWheel, gWheelFriction, gRollInfluence);


	connectionPointCS0 = Ogre::Vector3(
		-CUBE_HALF_EXTENTS+(0.3*gWheelWidth),
		connectionHeight,
		-2*CUBE_HALF_EXTENTS+gWheelRadius);

	isFrontWheel = false;
	mVehicle->addWheel(
		mWheelNodes[2],
		connectionPointCS0,
		wheelDirectionCS0,
		wheelAxleCS,
		gSuspensionRestLength,
		gWheelRadius,
		isFrontWheel, gWheelFriction, gRollInfluence);

	connectionPointCS0 = Ogre::Vector3(
		CUBE_HALF_EXTENTS-(0.3*gWheelWidth),
		connectionHeight,
		-2*CUBE_HALF_EXTENTS+gWheelRadius);

	mVehicle->addWheel(
		mWheelNodes[3],
		connectionPointCS0,
		wheelDirectionCS0,
		wheelAxleCS,
		gSuspensionRestLength,
		gWheelRadius,
		isFrontWheel, gWheelFriction, gRollInfluence);

	mVehicle->setWheelsAttached();


	
}

void MainApplication::setCamera(void)
{
	mCamera->setPosition(CameraStart);
    mCamera->lookAt(robotPosition);
}

void MainApplication::createFrameListener(void)
{
	BaseApplication::createFrameListener();

	// Start Bullet
	Ogre::Vector3 gravityVector = Ogre::Vector3(0,-9.81f,0);
	Ogre::AxisAlignedBox bounds = Ogre::AxisAlignedBox(Ogre::Vector3 (-10000, -10000, -10000), Ogre::Vector3 (10000,  10000,  10000));
	mWorld = new OgreBulletDynamics::DynamicsWorld(mSceneMgr, bounds, gravityVector);

	// add Debug info display tool
	debugDrawer = new OgreBulletCollisions::DebugDrawer();
	debugDrawer->setDrawWireframe(true);   // we want to see the Bullet containers
	mWorld->setDebugDrawer(debugDrawer);
	mWorld->setShowDebugShapes(true);      // enable it if you want to see the Bullet containers
	Ogre::SceneNode *node = mSceneMgr->getRootSceneNode()->createChildSceneNode("debugDrawer", Ogre::Vector3::ZERO);

}

bool MainApplication::frameStarted(const  Ogre::FrameEvent& evt)
{
    mWorld->stepSimulation(evt.timeSinceLastFrame);   // update Bullet Physics animation
    return BaseApplication::frameStarted(evt);
}

bool MainApplication::frameEnded(const  Ogre::FrameEvent& evt)
{
	mWorld->stepSimulation(evt.timeSinceLastFrame);   // update Bullet Physics animation
	return BaseApplication::frameEnded(evt);
}

bool MainApplication::frameRenderingQueued(const Ogre::FrameEvent& evt)
{
	// apply engine Force on relevant wheels
    for (int i = mWheelsEngine[0]; i < mWheelsEngineCount; i++)
    {
        mVehicle->applyEngineForce (mEngineForce, mWheelsEngine[i]);
    }

    if (mSteeringLeft)
    {
        mSteering += gSteeringIncrement;
        if (mSteering > gSteeringClamp)
            mSteering = gSteeringClamp;
    }
    else if (mSteeringRight)
    {
        mSteering -= gSteeringIncrement;
        if (mSteering < -gSteeringClamp)
            mSteering = -gSteeringClamp;
    }

	// apply Steering on relevant wheels
    for (int i = mWheelsSteerable[0]; i < mWheelsSteerableCount; i++)
    {
        if (i < 2)
            mVehicle->setSteeringValue (mSteering, mWheelsSteerable[i]);
        else
            mVehicle->setSteeringValue (-mSteering, mWheelsSteerable[i]);
    }

	return BaseApplication::frameRenderingQueued(evt);
}

bool MainApplication::keyPressed( const OIS::KeyEvent &arg )
{
	if(arg.key == OIS::KC_NUMPAD8)
	{
		//UP
		mEngineForce = gMaxEngineForce;
	}
	else if(arg.key == OIS::KC_NUMPAD2)
	{
		//DOWN
		mEngineForce = -gMaxEngineForce;
	}
	else if(arg.key == OIS::KC_NUMPAD6)
	{
		//RIGHT
		mSteeringRight = true;
	}
	else if(arg.key == OIS::KC_NUMPAD4)
	{
		//LEFT
		mSteeringLeft = true;
	}

	return BaseApplication::keyPressed(arg);
}

bool MainApplication::keyReleased( const OIS::KeyEvent &arg )
{
    if(arg.key == OIS::KC_NUMPAD8)
	{
		mEngineForce = 0;
	}
	else if(arg.key == OIS::KC_NUMPAD2)
	{
		mEngineForce = 0;
	}
	else if(arg.key == OIS::KC_NUMPAD6)
	{
		 mSteeringRight = false;
	}
	else if(arg.key == OIS::KC_NUMPAD4)
	{
		mSteeringLeft = false;
	}
    return BaseApplication::keyReleased(arg);
}

#ifdef __cplusplus
extern "C" {
#endif

#if OGRE_PLATFORM == OGRE_PLATFORM_WIN32
    INT WINAPI WinMain( HINSTANCE hInst, HINSTANCE, LPSTR strCmdLine, INT )
#else
    int main(int argc, char *argv[])
#endif
    {
        // Create application object
        MainApplication app;

        try {
            app.go();
        } catch( Ogre::Exception& e ) {
#if OGRE_PLATFORM == OGRE_PLATFORM_WIN32
            MessageBox( NULL, e.getFullDescription().c_str(), "An exception has occured!", MB_OK | MB_ICONERROR | MB_TASKMODAL);
#else
            std::cerr << "An exception has occured: " <<
                e.getFullDescription().c_str() << std::endl;
#endif
        }

        return 0;
    }

#ifdef __cplusplus
}
#endif
